#include "ulib.h"

struct ips_node_struct_t
{
	 union
	 {
		  struct
		  {
			   /* Currently not used */
			   int delta;
			   struct
			   {
					unsigned int wait    : 1;
					unsigned int ac_wait : 1;
					unsigned int pid     : 22;
					unsigned int prev    : 20;
					unsigned int next    : 20;
			   } __attribute__ ((packed));
		  } wq;
		  
		  uint32_t free_next;
	 };
};

#define IPS_NODE_SIZE 12
#define IPS_NODE_COUNT ((PAGETAB_SIZE) / IPS_NODE_SIZE)
#define IPS_NODE_START 1
struct proc_ln_t
{
	 spin_lock_t lock;
	 uint32_t    free_head;
};

static char __ips_size_assert[sizeof(ips_node_t) == IPS_NODE_SIZE ? 0 : -1];
static char __mutex_size_assert[sizeof(mutex_t) == 8 ? 0 : -1];
static char __sem_size_assert[sizeof(semaphore_t) == 12 ? 0 : -1];
static char __bq_size_assert[sizeof(bqueue_t) == 8 ? 0 : -1];

extern struct proc_ln_t lna;
extern ips_node_t ips_nodes[IPS_NODE_COUNT];

int
ips_init(void)
{
	 sc_alloc_temp_4M((void *)ULNA);

	 spl_init(&lna.lock);
	 lna.free_head = IPS_NODE_START;
	 int i;
	 for (i = IPS_NODE_START;
		  i != IPS_NODE_COUNT; ++ i)
	 {
		  ips_nodes[i].free_next = i + 1;
	 }
	 ips_nodes[IPS_NODE_COUNT - 1].free_next = 0;

	 sc_mark_shared_4M((void *)ULNA);
}

ips_node_t *
ips_alloc(void)
{
	 sc_spl_acquire(&lna.lock);
	 int result = lna.free_head;
	 if (result != 0)
		  lna.free_head = ips_nodes[result].free_next;
	 spl_release(&lna.lock);
	 return ips_nodes + result;
}

void
ips_free(ips_node_t *node)
{
	 sc_spl_acquire(&lna.lock);
	 int id = node - ips_nodes;
	 ips_nodes[id].free_next = lna.free_head;
	 lna.free_head = id;
	 spl_release(&lna.lock); 
}

#define IPS_ID(_ips) ((_ips) - ips_nodes)

void
mutex_init(mutex_t *mutex)
{
	 spl_init(&mutex->lock);
	 mutex->hold = 0;
}

int
mutex_try_acquire(mutex_t *mutex)
{
	 int result;
	 sc_spl_acquire(&mutex->lock);
	 if (mutex->hold == 0)
	 {
		  mutex->hold = 1;
		  mutex->wait = 0;
		  
		  result = 1;
	 }
	 else result = 0;
	 spl_release(&mutex->lock);
	 return result;
}

void
mutex_acquire(mutex_t *mutex, ips_node_t *node)
{
	 node->wq.pid     = cur_pid();
	 node->wq.ac_wait = 0;
	 
	 sc_spl_acquire(&mutex->lock);
	 if (mutex->hold == 0)
	 {
		  mutex->hold = 1;
		  mutex->wait = 0;
		  
		  spl_release(&mutex->lock);
	 }
	 else
	 {
		  if (mutex->wait)
		  {
			   node->wq.next = mutex->head;
			   node->wq.prev = ips_nodes[mutex->head].wq.prev;
			   ips_nodes[node->wq.next].wq.prev = IPS_ID(node);
			   ips_nodes[node->wq.prev].wq.next = IPS_ID(node);
		  }
		  else
		  {
			   mutex->wait = 1;
			   mutex->head = node->wq.next = node->wq.prev = IPS_ID(node);
		  }

		  sc_wait(&mutex->lock);
		  while (node->wq.wait && node->wq.ac_wait)
		  {
			   sc_wait(NULL);
		  }
	 }		  
}

void
mutex_ac_break(mutex_t *mutex, ips_node_t *node)
{
	 sc_spl_acquire(&mutex->lock);
	 if (node->wq.wait)
	 {
		  ips_nodes[node->wq.next].wq.prev = node->wq.next;
		  ips_nodes[node->wq.prev].wq.next = node->wq.prev;
		  if (mutex->head == IPS_ID(node))
		  {
			   if (node->wq.next == mutex->head)
					mutex->wait = 0;
			   else mutex->head = node->wq.next;
		  }
	 }
	 spl_release(&mutex->lock);
	 node->wq.ac_wait = 0;
}

void
mutex_release(mutex_t *mutex)
{
	 int notify = PROC_NULL;
	 if (!mutex->hold) return;
	 sc_spl_acquire(&mutex->lock);
	 if (mutex->wait)
	 {
		  ips_node_t *node = ips_nodes + mutex->head;
		  node->wq.wait = 0;
		  
		  notify = node->wq.pid;
		  ips_nodes[node->wq.next].wq.prev = node->wq.prev;
		  ips_nodes[node->wq.prev].wq.next = node->wq.next;
		  if (mutex->head == node->wq.next)
			   mutex->wait = 0;
		  else mutex->head = node->wq.next;
	 }
	 else mutex->hold = 0;
	 spl_release(&mutex->lock);
	 if (notify != PROC_NULL)
		  sc_notify(notify);
}

void
semaphore_init(semaphore_t *semaphore, uint32_t count)
{
	 spl_init(&semaphore->lock);
	 semaphore->count = count;
	 semaphore->wait = 0;
}

int
semaphore_try_acquire(semaphore_t *semaphore)
{
	 int result;
	 sc_spl_acquire(&semaphore->lock);
	 if (semaphore->count > 0)
	 {
		  if (-- semaphore->count == 0)
			   semaphore->wait = 0;
		  result = 1;
	 }
	 else result = 0;
	 spl_release(&semaphore->lock);
	 return result;
}

void
semaphore_acquire(semaphore_t *semaphore, ips_node_t *node)
{
	 node->wq.pid     = cur_pid();
	 node->wq.ac_wait = 0;
	 
	 sc_spl_acquire(&semaphore->lock);
	 if (semaphore->count > 0)
	 {
		  if (-- semaphore->count == 0)
			   semaphore->wait = 0;
		  
		  spl_release(&semaphore->lock);
	 }
	 else
	 {
		  if (semaphore->wait)
		  {
			   node->wq.next = semaphore->head;
			   node->wq.prev = ips_nodes[semaphore->head].wq.prev;
			   ips_nodes[node->wq.next].wq.prev = IPS_ID(node);
			   ips_nodes[node->wq.prev].wq.next = IPS_ID(node);
		  }
		  else
		  {
			   semaphore->wait = 1;
			   semaphore->head = node->wq.next = node->wq.prev = IPS_ID(node);
		  }

		  sc_wait(&semaphore->lock);
		  while (node->wq.wait && node->wq.ac_wait)
		  {
			   sc_wait(NULL);
		  }
	 }		  
}

void
semaphore_ac_break(semaphore_t *semaphore, ips_node_t *node)
{
	 sc_spl_acquire(&semaphore->lock);
	 if (node->wq.wait)
	 {
		  ips_nodes[node->wq.next].wq.prev = node->wq.next;
		  ips_nodes[node->wq.prev].wq.next = node->wq.prev;
		  if (semaphore->head == IPS_ID(node))
		  {
			   if (node->wq.next == semaphore->head)
					semaphore->wait = 0;
			   else semaphore->head = node->wq.next;
		  }
	 }
	 spl_release(&semaphore->lock);
	 node->wq.ac_wait = 0;
}

int
semaphore_release(semaphore_t *semaphore)
{
	 int result = 0;
	 int notify = PROC_NULL;
	 sc_spl_acquire(&semaphore->lock);
	 if (semaphore->wait)
	 {
		  ips_node_t *node = ips_nodes + semaphore->head;
		  node->wq.wait = 0;
		  
		  notify = node->wq.pid;
		  ips_nodes[node->wq.next].wq.prev = node->wq.prev;
		  ips_nodes[node->wq.prev].wq.next = node->wq.next;
		  if (semaphore->head == node->wq.next)
			   semaphore->wait = 0;
		  else semaphore->head = node->wq.next;

		  result = 1;
	 }
	 else
	 {
		  ++ semaphore->count;
	 }
	 spl_release(&semaphore->lock);
	 if (notify != PROC_NULL)
		  sc_notify(notify);

	 return result;
}

#define BQ_NOWAIT  0
#define BQ_WAITPUT 1
#define BQ_WAITGET 2

void
bqueue_init(bqueue_t *bq)
{
	 spl_init(&bq->lock);
	 bq->wait = BQ_NOWAIT;
}

void
bqueue_put(bqueue_t *bq, ips_node_t *node)
{	 
	 sc_spl_acquire(&bq->lock);
	 if (bq->wait == BQ_WAITPUT)
	 {
		  node = ips_nodes + bq->head;
		  node->wq.wait = 0;
		  
		  ips_nodes[node->wq.next].wq.prev = node->wq.prev;
		  ips_nodes[node->wq.prev].wq.next = node->wq.next;
		  if (bq->head == node->wq.next)
			   bq->wait = BQ_NOWAIT;
		  else bq->head = node->wq.next;

		  spl_release(&bq->lock);
		  sc_notify(node->wq.pid);
	 }
	 else
	 {
		  node->wq.pid     = cur_pid();
		  node->wq.ac_wait = 0;

		  if (bq->wait == BQ_WAITGET)
		  {
			   node->wq.next = bq->head;
			   node->wq.prev = ips_nodes[bq->head].wq.prev;
			   ips_nodes[node->wq.next].wq.prev = IPS_ID(node);
			   ips_nodes[node->wq.prev].wq.next = IPS_ID(node);
		  }
		  else
		  {
			   bq->wait = BQ_WAITGET;
			   bq->head = node->wq.next = node->wq.prev = IPS_ID(node);
		  }

		  sc_wait(&bq->lock);
		  while (node->wq.wait && node->wq.ac_wait)
		  {
			   sc_wait(NULL);
		  }
	 }		  
}

void
bqueue_get(bqueue_t *bq, ips_node_t *node)
{	 
	 sc_spl_acquire(&bq->lock);
	 if (bq->wait == BQ_WAITGET)
	 {
		  node = ips_nodes + bq->head;
		  node->wq.wait = 0;
		  
		  ips_nodes[node->wq.next].wq.prev = node->wq.prev;
		  ips_nodes[node->wq.prev].wq.next = node->wq.next;
		  if (bq->head == node->wq.next)
			   bq->wait = BQ_NOWAIT;
		  else bq->head = node->wq.next;

		  spl_release(&bq->lock);
		  sc_notify(node->wq.pid);
	 }
	 else
	 {
		  node->wq.pid     = cur_pid();
		  node->wq.ac_wait = 0;

		  if (bq->wait == BQ_WAITPUT)
		  {
			   node->wq.next = bq->head;
			   node->wq.prev = ips_nodes[bq->head].wq.prev;
			   ips_nodes[node->wq.next].wq.prev = IPS_ID(node);
			   ips_nodes[node->wq.prev].wq.next = IPS_ID(node);
		  }
		  else
		  {
			   bq->wait = BQ_WAITPUT;
			   bq->head = node->wq.next = node->wq.prev = IPS_ID(node);
		  }

		  sc_wait(&bq->lock);
		  while (node->wq.wait && node->wq.ac_wait)
		  {
			   sc_wait(NULL);
		  }
	 }		  
}
